#include <stdio.h>
#include <time.h>
#include <sys/time.h>

#include "myservo.h"
#include "ethercat.h"
#include "osal.h"
#include "oshw.h"

typedef struct
{
	X3E_Driver servo_data[1];
}SERVOS_T;

volatile int wkc;

static char IOmap[IOMAP_SIZE];
SERVOS_T *hservos = (SERVOS_T *)IOmap;
enum DriverMode X3E_Mode;
static unsigned int op_once = 1;

/*通过osal确定时间*/
void echo_time(void)
{
	struct timeval tp;
	//osal_gettimeofday(&tp, 0);
	gettimeofday(&tp, 0);
	int a,b,c;
	a=tp.tv_sec;
	b=tp.tv_usec/1000;
	c=tp.tv_usec%1000;
	printf("****cur time = %d,%03d,%03d(us)\r\n", a, b, c);
	//printf("****cur time = %d,%03d,%03d(us)\r\n", tp.tv_sec, tp.tv_usec/1000, tp.tv_usec%1000);
}

void hex_dump(char *ptr, unsigned int buflen)
{
	unsigned char *buf = (unsigned char *)ptr;
	unsigned int i, j;
	printf("\r\nRPDO\r\n");
	for (i = 0; i < 55; i++)
	{printf("%02X ", buf[i]);}
	printf("\r\nTPDO\r\n");
	for (j = 55; j < buflen; j++)
	{printf("%02X ", buf[j]);}
}

void view_slave_data(void)
{
	hex_dump(IOmap,70);
}

/*只访问单个子索引写*/
int SGL_SDOwrite(uint16_t Slave, uint16_t Index, uint8_t SubIndex, int size, void *data)
{
	int wkc, cnt=0;
	do{
		wkc = ec_SDOwrite(Slave, Index, SubIndex, FALSE, size, data, EC_TIMEOUTRXM);
		cnt++;
	}while(wkc<=0 && cnt<10);
	return wkc;
}

int SGL_SDOwrite_b(uint16_t Slave, uint16_t Index, uint8_t SubIndex, uint8_t data)
{
	return SGL_SDOwrite(Slave, Index, SubIndex, 1, &data);
}

int SGL_SDOwrite_w(uint16_t Slave, uint16_t Index, uint8_t SubIndex, uint16_t data)
{
	return SGL_SDOwrite(Slave, Index, SubIndex, 2, &data);
}

int SGL_SDOwrite_dw(uint16_t Slave, uint16_t Index, uint8_t SubIndex, uint32_t data)
{
	return SGL_SDOwrite(Slave, Index, SubIndex, 4, &data);
}

/*只访问单个子索引读*/
int SGL_SDOread(uint16_t Slave, uint16_t Index, uint8_t SubIndex, int size, void *data)
{
	int wkc, cnt=0;
	do{
		wkc = ec_SDOread(Slave, Index, SubIndex, FALSE, &size, data, EC_TIMEOUTRXM);
	}while(wkc<=0 && cnt<10);
	return wkc;
}

int SGL_SDOread_b(uint16_t Slave, uint16_t Index, uint8_t SubIndex, uint8_t data)
{
	return SGL_SDOread(Slave, Index, SubIndex, 1, &data);
}

int SGL_SDOread_w(uint16_t Slave, uint16_t Index, uint8_t SubIndex, uint16_t data)
{
	return SGL_SDOread(Slave, Index, SubIndex, 2, &data);
}

int SGL_SDOread_dw(uint16_t Slave, uint16_t Index, uint8_t SubIndex, uint32_t data)
{
	return SGL_SDOread(Slave, Index, SubIndex, 4, &data);
}


/*PDO配置*/
void process_data_config(void)
{
	uint8_t SI;
	for(int slave = 1; slave <= *ecx_context.slavecount; slave++)
	{
		//RPDO mapping------------
		//1c12.0
		SGL_SDOwrite_b(slave, 0x1c12, 0, 0);
		SGL_SDOwrite_w(slave, 0x1c12, 1, htoes(0x1600));
		
		//1600.0
		SI = 0;
		SGL_SDOwrite_b(slave, 0x1600, 0, 0);
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60400010));//6040h(控制字)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60600008));//6060h(控制模式)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60710010));//6071h(目标扭矩)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x607a0020));//607Ah(目标位置)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60800020));//6080h(最大电机速度)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x607F0020));//607Fh(最大轮廓速度)
		//SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60B80010));//60B8h(探针功能)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60ff0020));//60ffh(目标速度)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60b10020));//60b1h(速度偏置)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60b20010));//60b2h(扭矩偏置)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60e00010));//60e0h(正向最大转矩限制)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60e10010));//60e1h(负向最大转矩限制)
		//SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x607E0008));//607Eh(指令极性,重新上电后有效)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x608f0120));//608f01h(电机编码器分辨率)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x608f0220));//608f02h(电机分辨率对应电机旋转圈数)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60910120));//609101h(电子齿轮比：分子)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60910220));//609102h(电子齿轮比：分母)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60920120));//609201h(给进常量：分子)
		SGL_SDOwrite_dw(slave, 0x1600, ++SI, htoel(0x60920220));//609202h(给进常量：分母)
		SGL_SDOwrite_b(slave, 0x1600, 0, SI);

		//1c12.0
		SGL_SDOwrite_b(slave, 0x1c12, 0, 1);


		//TPDO mapping------------
		//1c13.0
		SGL_SDOwrite_b(slave, 0x1c13, 0, 0);
		SGL_SDOwrite_w(slave, 0x1c13, 1, htoes(0x1a00));

		//1a00.0
		SI = 0;
		SGL_SDOwrite_b(slave, 0x1a00, 0, 0);
		SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x603F0010));//603Fh(错误码)
		SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60410010));//6041h(状态字)
		SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60610008));//6061h(控制模式显示)
		SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60640020));//6064h(位置反馈)
		SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x606c0020));//606ch(速度反馈)
		SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60770010));//6064h(转矩反馈)
		//SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60B90010));//60B9h(探针状态)
		//SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60BA0020));//60BAh(探针1上升沿位置反馈)
		//SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60BB0020));//60BBh(探针1下降沿位置反馈)
		//SGL_SDOwrite_dw(slave, 0x1a00, ++SI, htoel(0x60FD0020));//60FDh(DI输入状态)
		SGL_SDOwrite_b(slave, 0x1a00, 0, SI);

		//1c13.0
		SGL_SDOwrite_b(slave, 0x1c13, 0, 1);

		//初始控制模式CSV
		//X3E_Mode = CSV;
		//SGL_SDOwrite_b(slave, 0x6060, 0, X3E_Mode);	
	}
}


//适用于禾川SVX3 
/* 流程       			control_word（下发）            status_word bit0~9(回告,取与判断)
 * 初始化中													0x0000
 * 初始化完成等待											0x0250/0x0270
 * 切入伺服准备好(上电)			0x0006						0x0231
 * 伺服使能						0x0007						0x0233
 * 伺服运行						0x000F						0x0237
 * 停止运行						0x0000						 0x0250
 * 
 * 从运行中到断开主回路必须按上电→使能→运行的相反顺序进行
 * 
 * 运行中快速停机				0x0002						0x0217
 * 
 * 伺服故障（自动停机转故障）								 0x021F
 * 故障														0x0218
 * 故障清除						0x0080						0x0250
 */


//直接到op模式状态切换步骤,支持初始化→op；停机中→op
void servo_switch_op(void)
{
	uint32_t sta;
	for(int slave = 1; slave <= *ecx_context.slavecount; slave++)
	{
		uint32_t idx = slave - 1;
		sta = hservos->servo_data[idx].DriverStatus.StatusWord;//读取状态字bit13~bit0 
		if(sta & 0x08)//如果有故障
		{
			hservos->servo_data[idx].DriverControl.ControlWord = 0x80;
			continue;
		}
		else if (1 == op_once)
		{
			if(0x40 == (sta& 0x4F))//未使能
			{
				hservos->servo_data[idx].DriverControl.ControlWord = 0x06;
				X3E_Mode = CSV;
				hservos->servo_data[0].DriverControl.Mode = X3E_Mode;
			}

			if(0x21 == (sta& 0x6F))//准备好
			{
			    hservos->servo_data[idx].DriverControl.ControlWord = 0x07;
			}

			if(0x23 == (sta& 0x6F))//已使能
			{
			    hservos->servo_data[idx].DriverControl.ControlWord = 0x0F;
				op_once = 0;
			}
			printf("running!sta= %x.\n", sta);
		}	 
	}
}

//停止运行
void servo_switch_idle(void)
{
	uint32_t sta;
	for(int slave = 1; slave <= *ecx_context.slavecount; slave++)
	{
		uint32_t idx = slave - 1;
		sta = hservos->servo_data[idx].DriverStatus.StatusWord;//读取状态字bit13~bit0
		if(0x27 == (sta& 0x6F))//运行中
		{
        	hservos->servo_data[idx].DriverControl.ControlWord = 0x00;
			printf("stopping!sta= %x.\n", sta);
		}
	}
}

//
void SVX3E_config(char *ifname)
{
	ecx_context.manualstatechange = 1;

	if (ec_init(ifname))
	{
		printf("ec_init on %s succeeded.\r\n",ifname);

		/* request Init for slave(可能非必要)*/
		printf("\nRequest init state for all slaves\r\n");
		ec_slave[0].state = EC_STATE_INIT;
		ec_writestate(0);
		ec_readstate();
		/* wait for all slaves to reach Init state */
		ec_statecheck(0, EC_STATE_INIT,  EC_TIMEOUTSTATE * 3);

		if (ec_slave[0].state != EC_STATE_INIT )
		{
			printf("Not all slaves reached init state.\r\n");
			ec_readstate();
			for(int i = 1; i<=ec_slavecount ; i++)
			{
				if(ec_slave[i].state != EC_STATE_INIT)
				{
					//printf("Slave %d State=0x%2x StatusCode=0x%04x : %s\r\n", i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
				}
			}
		}

		//echo_time();
		//if ( ec_config(FALSE, &IOmap) > 0 )

		if ( ec_config_init(FALSE) > 0 )
		{
			//ec_configdc();
			//ec_dcsync0(1, TRUE, 1000000, 0); // SYNC0 on slave 1
			
			while(EcatError) printf("%s", ec_elist2string());
			printf("%d slaves found and configured.\r\n",ec_slavecount);

			/* request pre_op for slave(可能非必要)*/
			printf("Request pre_op state for all slaves\r\n");
			ec_slave[0].state = EC_STATE_PRE_OP | EC_STATE_ACK;
			ec_writestate(0);
			
			//现在应该在pre_op状态
			process_data_config();

			/*FMMU IOMAP*/
			ec_config_map(IOmap);
			printf("Slaves mapped, state to SAFE_OP.\r\n");

			ec_configdc();

			/* request safe_op for slave */
			ec_slave[0].state = EC_STATE_SAFE_OP;
			ec_writestate(0);

			/* wait for all slaves to reach SAFE_OP state */
            ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 3);

			//printf("DC capable:%d\r\n",ec_configdc());

			if (ec_slave[0].state != EC_STATE_SAFE_OP )
			{
				printf("Not all slaves reached safe_op state.\r\n");
				ec_readstate();
				for(int i = 1; i<=ec_slavecount ; i++)
				{
					if(ec_slave[i].state != EC_STATE_SAFE_OP)
					{
						//printf("Slave %d State=0x%2x StatusCode=0x%04x : %s\r\n", i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
					}
				}
			}
			else
			{
				hservos->servo_data[0].DriverControl.max_Speed=300000;
				hservos->servo_data[0].DriverControl.max_Speed_outline=200000;
				hservos->servo_data[0].DriverControl.Torque_Polarity =0;
				hservos->servo_data[0].DriverControl.Speed_Polarity =0;
				hservos->servo_data[0].DriverControl.Torque_Limit_P=1000;
				hservos->servo_data[0].DriverControl.Torque_Limit_N=1000;
				//hservos->servo_data[0].DriverControl.Encoder_num=131072;//默认17bit
				//hservos->servo_data[0].DriverControl.Encoder_deno=1;
				//hservos->servo_data[0].DriverControl.Gear_num=1;
				//hservos->servo_data[0].DriverControl.Gear_deno=1;
				//hservos->servo_data[0].DriverControl.Feed_num=1;
				//hservos->servo_data[0].DriverControl.Feed_deno=1;
				
				printf("Request operational state for all slaves\r\n");
				//int expectedWKC;
				//expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
				
				//先发一次初始化SM
				ec_send_processdata();
				ec_receive_processdata(EC_TIMEOUTRET);
				
				/* request OP for slave */
				ec_slave[0].state = EC_STATE_OPERATIONAL;
				/* send one valid process data to make outputs in slaves happy*/
				ec_send_processdata();
				wkc=ec_receive_processdata(EC_TIMEOUTRET);
				//printf("--->workcounter %d\r\n", wkc);
				//view_slave_data();
				ec_writestate(0);
				ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
				//ec_readstate();

				/* wait for all slaves to reach OP state */
				int cnt = 200;
				do
				{
					ec_slave[0].state = EC_STATE_OPERATIONAL;
					ec_send_processdata();
					ec_receive_processdata(EC_TIMEOUTRET);
				  	//printf("--->workcounter %d\r\n", wkc);//刷屏
					ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
					//echo_time();
				}
				while (cnt-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));

				if (ec_slave[0].state == EC_STATE_OPERATIONAL )
				{
					printf("All slaves reached Operational state.\r\n");

					osal_timert t;
					osal_timer_start(&t, 1000);
					ec_dcsync0(1, TRUE, 1000000, 0); // SYNC0 on slave 1
					for(int i = 1; i <= 10000; i++)	//内部强制停制时间，不应大于main的测试时间，否则总线故障，10000=10s
					{
						servo_switch_op();
						if(hservos->servo_data[0].DriverControl.ControlWord == 0x07)
						{
							hservos->servo_data[0].DriverControl.Target_Speed =0;
						}
						if(hservos->servo_data[0].DriverControl.ControlWord == 0x0f)
						{
							hservos->servo_data[0].DriverControl.Target_Speed =131072;
						}

						while(osal_timer_is_expired(&t)==FALSE);
						osal_timer_start(&t, 1000);

						ec_send_processdata();
						wkc = ec_receive_processdata(EC_TIMEOUTRET);
						//view_slave_data();
					}
					//view_slave_data();
					//循环结束停止,可以用00或02
					ec_dcsync0(1, FALSE, 2000000, 0); // SYNC0 on slave 1
					osal_timer_start(&t, 1000);
					while(osal_timer_is_expired(&t)==FALSE);
					hservos->servo_data[0].DriverControl.Target_Speed =0;
					hservos->servo_data[0].DriverControl.Speed_Polarity=0;
					hservos->servo_data[0].DriverControl.ControlWord =0x00;
					ec_send_processdata();
					ec_receive_processdata(EC_TIMEOUTRET);
					printf("emergency stop!\r\n");
				}
				else
				{
					printf("Not all slaves reached operational state.\r\n");
					ec_readstate();
					for(int i = 1; i<=ec_slavecount ; i++)
					{
						if(ec_slave[i].state != EC_STATE_OPERATIONAL)
						{
							//printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\r\n",i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
						}
					}
				}
				
				printf("\nRequest init state for all slaves\r\n");
				ec_slave[0].state = EC_STATE_INIT;
				ec_writestate(0);
			}
		}
		else
		{
			printf("No slaves found!\r\n");
		}
		printf("End soem, close socket\r\n");
		ec_close();
	}
	else
	{
		printf("ec_init on %s failed.\r\n",ifname);
	}
	//printf("IOMAP addr = 0x%08x\r\n", (uint32_t)IOmap);
	//printf("========================\r\n");
	//view_slave_data();
}
